﻿using System;
using System.Collections.Generic;
using System.Text;
using BarStructures.Deformed;
using MathNet.Numerics.LinearAlgebra;
using BarStructures.Utils;

namespace BarStructures.Analysers
{
    /// <summary>
    /// Блочная динамическая конденсация с дискретной матрицей масс.
    /// </summary>
    public class BlockDynamicCondensation_XY_DM: Analyser
    {
        private Matrix eigenVectors = null;

        private Vector eigenValues = null;

        int[] mergedDegrees = null;

        /// <summary> Размер блока из группы второстепенных степеней свободы. </summary>
        private int groupSize = 4;

        public BlockDynamicCondensation_XY_DM(Model model, string name)
            : base(model, name)
        {

        }

        public override void Analyse()
        {
            if (!UseExistingIndexation)
                model.SetAutoSystemIndexes();

            int modelDimension = model.GetNodesCount() * 2;

            //подготовка матриц
            Matrix stiffness = new Matrix(modelDimension, modelDimension);
            Matrix mass = new Matrix(modelDimension, modelDimension);

            #region Построение матриц жесткости и масс
            Matrix elMx, save;
            int barsCount, ind1, ind2;
            barsCount = model.GetBarsCount();
            for (int i = 0; i < barsCount; i++)
            {
                Bar bar = model.GetBar(i);
                ind1 = bar.NodeOne.SystemIndex * 2;
                ind2 = bar.NodeTwo.SystemIndex * 2;

                elMx = ElementHelper.GetGlobalStiffness6DMatrix(bar);
                save = stiffness.GetMatrix(ind1, ind1 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(0, 1, 0, 1));
                stiffness.SetMatrix(ind1, ind1 + 1, ind1, ind1 + 1, save);

                save = stiffness.GetMatrix(ind1, ind1 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(0, 1, 3, 4));
                stiffness.SetMatrix(ind1, ind1 + 1, ind2, ind2 + 1, save);

                save = stiffness.GetMatrix(ind2, ind2 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(3, 4, 0, 1));
                stiffness.SetMatrix(ind2, ind2 + 1, ind1, ind1 + 1, save);

                save = stiffness.GetMatrix(ind2, ind2 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(3, 4, 3, 4));
                stiffness.SetMatrix(ind2, ind2 + 1, ind2, ind2 + 1, save);


                elMx = ElementHelper.GetGlobalDescreteMass6DMatrix(bar);
                save = mass.GetMatrix(ind1, ind1 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(0, 1, 0, 1));
                mass.SetMatrix(ind1, ind1 + 1, ind1, ind1 + 1, save);

                save = mass.GetMatrix(ind1, ind1 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(0, 1, 3, 4));
                mass.SetMatrix(ind1, ind1 + 1, ind2, ind2 + 1, save);

                save = mass.GetMatrix(ind2, ind2 + 1, ind1, ind1 + 1);
                save.Add(elMx.GetMatrix(3, 4, 0, 1));
                mass.SetMatrix(ind2, ind2 + 1, ind1, ind1 + 1, save);

                save = mass.GetMatrix(ind2, ind2 + 1, ind2, ind2 + 1);
                save.Add(elMx.GetMatrix(3, 4, 3, 4));
                mass.SetMatrix(ind2, ind2 + 1, ind2, ind2 + 1, save);
            }

            //DbgPrint.Matrix(mass, "mass_0.txt");
            //DbgPrint.Matrix(stiffness, "stiffness_0.txt"); 
            #endregion

            #region Индексы основных и второстепенных степеней свободы и смешанные индексы

            List<int> primaryDegreesList = new List<int>();
            List<int> secondaryDegreesList = new List<int>();
            for (int i = 0; i < model.GetNodesCount(); i++)
            {
                Node node = model.GetNodeBySystemIndex(i);
                if (node.DegreeSelection.X)
                    primaryDegreesList.Add(i * 2);
                else
                    if (!node.Support.Sx)
                        secondaryDegreesList.Add(i * 2);
                if (node.DegreeSelection.Y)
                    primaryDegreesList.Add(i * 2 + 1);
                else
                    if (!node.Support.Sy)
                        secondaryDegreesList.Add(i * 2 + 1);
            }
            int[] primaryDegrees = primaryDegreesList.ToArray();
            int[] secondaryDegrees = secondaryDegreesList.ToArray();
            mergedDegrees = new int[primaryDegrees.Length + secondaryDegrees.Length];
            Array.Copy(primaryDegrees, mergedDegrees, primaryDegrees.Length);
            Array.Copy(secondaryDegrees, 0, mergedDegrees, primaryDegrees.Length, secondaryDegrees.Length);

            int primaryLength = primaryDegrees.Length;
            int secondaryLength = secondaryDegrees.Length;
            int mergedLength = mergedDegrees.Length;

            #endregion

            //Рабочие матрицы, без связанных степеней свободы
            Matrix Kmrg = stiffness.GetMatrix(mergedDegrees, mergedDegrees).Inverse();
            Matrix Mmrg = mass.GetMatrix(mergedDegrees, mergedDegrees);

            //DbgPrint.Matrix(Kmrg, "stiffness.txt");
            //DbgPrint.Matrix(Mmrg, "mass.txt"); 

            //подматрицы от основных степеней свободы
            Matrix stKrr = Kmrg.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1);
            Matrix stMrr = Mmrg.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1);
            
            //объявление переменных подматриц для расчётов
            Matrix stKrs, stKsr, stKss, stMss;
            Matrix stK, stM;
            Matrix accomulatedDrs = Matrix.Zeros(primaryLength);

            //порядковые индексы начала и конца блока в группе второстепенных степеней свободы
            int blockStart, blockEnd;
            blockStart = primaryLength;
            blockEnd = blockStart + groupSize - 1;
            if (blockEnd >= mergedLength)
                blockEnd = mergedLength - 1;
            //блочная конденсация
            while (blockStart < mergedLength)
            {
                stKrs = Kmrg.GetMatrix(0, primaryLength - 1, blockStart, blockEnd);
                stKsr = Kmrg.GetMatrix(blockStart, blockEnd, 0, primaryLength - 1);
                stKss = Kmrg.GetMatrix(blockStart, blockEnd, blockStart, blockEnd);
                stMss = Mmrg.GetMatrix(blockStart, blockEnd, blockStart, blockEnd);

                stK = new Matrix(primaryLength + blockEnd - blockStart + 1, primaryLength + blockEnd - blockStart + 1);
                stM = new Matrix(primaryLength + blockEnd - blockStart + 1, primaryLength + blockEnd - blockStart + 1);

                stK.SetMatrix(0, primaryLength - 1, 0, primaryLength - 1, stKrr);
                stK.SetMatrix(0, primaryLength - 1, primaryLength, stK.ColumnCount - 1, stKrs);
                stK.SetMatrix(primaryLength, stK.RowCount - 1, 0, primaryLength - 1, stKsr);
                stK.SetMatrix(primaryLength, stK.RowCount - 1, primaryLength, stK.ColumnCount - 1, stKss);
                stM.SetMatrix(0, primaryLength - 1, 0, primaryLength - 1, stMrr);
                stM.SetMatrix(primaryLength, stM.RowCount - 1, primaryLength, stM.ColumnCount - 1, stMss);

                //собственные значения и усечённые векторы для подсистемы
                save = stK.Multiply(stM);
                EigenvalueDecomposition _evd = new EigenvalueDecomposition(save);
                Matrix _eigenValues = _evd.BlockDiagonal.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1); 
                Matrix _eigenVectors = _evd.EigenVectors.GetMatrix(0, primaryLength - 1, 0, primaryLength - 1);

                //нормирование усечённых векторов
                for (int i = 0; i < _eigenVectors.ColumnCount; i++)
                {
                    double norm = 0;
                    for (int j = 0; j < _eigenVectors.RowCount; j++)
                        norm += _eigenVectors[i, j] * _eigenVectors[i, j];
                    norm = Math.Sqrt(norm);
                    for (int j = 0; j < _eigenVectors.RowCount; j++)
                        _eigenVectors[i, j] /= norm;
                }

                Matrix Drs = _eigenVectors.Multiply(_eigenValues).Multiply(_eigenVectors.Inverse());
                accomulatedDrs.Add(Drs);

                blockStart += groupSize;
                blockEnd += groupSize;
                if (blockEnd >= mergedLength)
                    blockEnd = mergedLength - 1;
            }

            //полная матрица кондесационных добавок
            int groupCount = (int)Math.Ceiling((double)(secondaryLength / groupSize));
            Matrix _save = stMrr.Multiply(stKrr);
            _save.Multiply(-groupCount + 1);
            accomulatedDrs.Add(_save);

            EigenvalueDecomposition evd = new EigenvalueDecomposition(accomulatedDrs);
            this.eigenValues = evd.RealEigenvalues;
            this.eigenVectors = evd.EigenVectors;
        }

        public override DeformedState GetDeformedStateForEigenValue(double eigenValue)
        {
            throw new NotImplementedException();
        }

        public override List<double> GetEigenValuesList()
        {
            if (eigenValues == null)
                Analyse();
            List<double> result = new List<double>();
            for (int i = 0; i < eigenValues.Length; i++)
                result.Add(1/eigenValues[i]);
            return result;
        }

        public override List<double> GetEigenVecsListForEigenValue(double eigenValue)
        {
            throw new NotImplementedException();
        }

        public override List<double> GetFullEigenVecsListForEigenValue(double eigenValue)
        {
            throw new NotImplementedException();
        }
    }
}
